/* @FileName:peripheral.h
 * @Author: haibo.yang
 * @Date: 2022-09-1  08:55:14
 * @Last Modified by: haibo.yang
 * @Last Modified time: 2022-09-1 11:17:39
 */
#pragma once

#include "rclcpp/rclcpp.hpp"
// #include <dynamic_reconfigure/server.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64.hpp>
// #include <canopen_chain_node/SetObject.h>
#include <agv_actions/PeripheralAction.h>
#include <agv_msgs/msg/v_action.hpp>
#include <actionlib/server/simple_action_server.h>
#include <std_msgs/msg/u_int16.hpp>
#include <agv_msgs/msg/periph_status.hpp>
#include <agv_msgs/msg/periph_teleop.hpp>
#include <agv_msgs/msg/agv_mode.hpp>
#include <agv_msgs/msg/event.hpp>
#include <agv_msgs/msg/event_occur.hpp>
#include <agv_msgs/msg/v_load.hpp>
#include <agv_msgs/msg/drivers_info.hpp>
#include <agv_msgs/msg/set_stop_bits.hpp>
#include <agv_msgs/msg/agv_auto_status.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include <agv_srvs/srv/outputs_new.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <vector>
// #include <agv_peripheral_controller/periphDynParamConfig.h>
#include <controller_manager_msgs/srv/list_controllers.hpp>
#include <controller_manager_msgs/msg/controller_state.hpp>
#include <yaml-cpp/yaml.h>
#include <thread>
#include <mutex>
#include <fstream>
#include <ros/package.h>

#define JOINT_NAME_PREFIX "jacking_joint_"
#define PROVELOBJ "6081"
#define OPERATE_TIMEOUT_S (10)
#define MOVE_TIMEOUT_S (60)
#define PACKAGE_NAME "agv_peripheral_controller"
#define FILE_NAME "/params/posRecord.yaml"


#define AGV_PERIPHERAL_CONTROLLER_NODE_ID   (2)



namespace agv_peripheral
{

  class PalletController;

  class AgvPeripheral
  {
  public:
    enum Type
    {
      JACKING,
      ROTARY,
      JACKING_ROTARY,
      ROLLER,
      FORKLIFT,
      ZAPI_CART,
      YUFENG_CART,
      MULTI_JACK,
    } peripheral_type;

    AgvPeripheral(const std::string &name);

    ~AgvPeripheral();

    bool init();
    void spin();
    void shutdown() { shutdown_req = true; };
    void preemptCB();
    void goalCB();
    void periphStateMachineUpdate();
    void periphStatusGather();
    bool periphMotorErrCheck();
    bool periphMotorEnableCheck();
    bool periphHomingCheck();
    bool periphReadyCheck();
    bool periphGetControllerRunningStatus();

  protected:
    rclcpp::Node nh; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
    actionlib::SimpleActionServer<agv_actions::PeripheralAction> as;
    agv_actions::PeripheralFeedback feedback;
    agv_actions::PeripheralResult result;


  private:
    std::string name;
    int jacking_joint_num;
    double frequency;
    double jacking_velocity;
    bool shutdown_req;
    bool load_with_qrcode;
    std::vector<int> joint_ids;
    bool find_joint_id;
    bool periphInitialized;
    // int jacking_speed;
    rclcpp::Time operate_time;
    rclcpp::Time jointState_update_time;
    bool followup_flag;
    int  bit_pos_rotate = 8;
      
    std::string action_pick = "pick";
    std::string action_drop = "drop";
    std::string action_adjust = "lhdAdjust";

    bool jacking_move_state = false;
    bool jacking_arrive_recordPos = false;
    rclcpp::Time jacking_move_time;

    std::vector<uint16_t>   last_jacking_status_word;
    std::vector<bool>       last_jacking_err;

    rclcpp::Time err_begin_time;

    bool manual_jacking_active;
    double jacking_timeout_time;
    rclcpp::Time last_jacking_time;
    rclcpp::Time last_landmark_shelf_time;
    rclcpp::Time last_event_time;
    rclcpp::Time last_set_stop_time;

    /*** global variables ***/
    sensor_msgs::msg::JointState joint_state; /**< joint state */
    std::mutex m_mutex;
    // float manual_target_pos[2];      /**< manual ctl mode position */
    std::vector<agv_msgs::msg::PeriphMotorInfo> periphMotor;
    agv_msgs::msg::PeriphStatus periphStatus;
    agv_msgs::msg::Event  periphEvent;
    agv_msgs::msg::EventOccur eventOccur;
    agv_msgs::msg::Load loadInfo;
    std::vector<agv_msgs::msg::Load> loads;
    agv_msgs::msg::DriversInfo driversInfo;
    agv_msgs::msg::SetStopBits setStop;

    std_srvs::srv::Trigger trig_srv;
    // canopen_chain_node::SetObject setobject;
    
    agv_msgs::msg::Action current_action;

    std::thread m_thread;
    std::timed_mutex m_quit_mutex;

    ros::Subscriber js_sub;    /**< joint state from hardware interface */

    ros::ServiceClient PeripheralHaltCtl; /**< joint point halt contrl */
    ros::ServiceClient PeripheralRecoverCtl; /**< joint point recover contrl */
    ros::ServiceClient peripheralGetcontrollers;
    ros::ServiceClient outputClient;

    ros::Publisher  periph_status_pub;
    ros::Subscriber periph_teleop_sub;
    ros::Subscriber agvInfo_sub; 
    ros::Subscriber agvMode_sub; 
    ros::Subscriber landmark_shelf_sub; 
    ros::Publisher  event_pub;
    ros::Publisher  load_pub;
    ros::Subscriber driver_info_sub;
    ros::Publisher  set_stop_pub;
    ros::Subscriber auto_status_sub;
    ros::Subscriber readinput_sub;
    std::vector<ros::Publisher>  jacking_pubs;

    /*********************
    ** Dynamic Reconfigure
    **********************/
    // dynamic_reconfigure::Server<agv_peripheral_controller::periphDynParamConfig> dynamic_reconfigure_server;
    // dynamic_reconfigure::Server<agv_peripheral_controller::periphDynParamConfig>::CallbackType dynamic_reconfigure_callback;

    // void reconfigCB(agv_peripheral_controller::periphDynParamConfig &config, uint32_t unused_level);
    void jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &ms);
    bool executeOP(int order);
    void periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg);
    // void agvInfocb(const agv_msgs::msg::AgvInfo::ConstSharedPtr &msg);
    void agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg);
    void landmarkShelfCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &data);
    void driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &msg);
    void autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg);
    void readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg);

    static void thread_fun(AgvPeripheral *pThis);
    void thread_cycle();
    void periphPosRecord();
    bool periphRecordPosRead();
    bool periphRecordPosWrite();
    bool periphMoveToRecordPos();
    void check_jointState_out();
    void set_stop_bits();
    void periphRecoverDetect();
    void setTargetJackingVelocitys(double vel);

    enum periph_op_enum
    {
      NO_OP,
      UP,
      DOWN,
    };

    enum periph_stata_enum
    {
      UNCONTROLLED_STATE = -1,
      IDLE_STATE,
      JACKING_OP_STATE_UP,
      JACKING_OP_STATE_DOWN,
      JACKING_SUCCESS_STATE,
      MANUAL_MODE_STATE,
      HALT_HANDLE_STATE,
    };

    enum periph_result_enum
    {

      SUCCESS,
      FAILED,
      CANCELED,
      OTHER,
    };


    struct OperateState
    {
      int cur_state;
      int cur_op;
      int next_op;
    } os;

    struct peripheral_motor_status
    {
      bool enableState;
      bool homeState;
      bool motorErr;
      bool movingState;
      bool ready;
    };

    std::vector<peripheral_motor_status> motorStatus;

    struct periph_read_input
    {
      bool manualState;
      bool reset;
      bool emergency;
      bool upControl;
      bool downControl;
      std::vector<bool> upLimit;
      std::vector<bool> downLimit;
      std::vector<bool> upMachLimit;
      std::vector<bool> downMachLimit;
    }read_input_data, last_read_input_data;

    bool jacking_up_manual;
    bool jacking_down_manual;
    

  public:
    void set_cur_op(periph_op_enum op);
    int get_cur_op();
    void set_next_op(periph_op_enum op);
    int get_next_op();
    void set_cur_state(periph_stata_enum op);
    int get_cur_state();
    void clear_os();


  };
} // agv_peripheral